"""
Boxplot_Path_Lengths

Purpose:
    This script analyzes and compares path lengths across multiple goals using different 
    algorithms (with and without Local Trajectory Refinement, LTR). It processes `.pkl` files 
    in the `test_pkl` folder to calculate the path lengths and generates a boxplot visualization 
    to highlight the differences in performance. The boxplot is saved in the `boxplot` folder 
    with a timestamped filename.

Functionality:
1. Reads all `.pkl` files in the `test_pkl` folder and groups them into batches of 10 files per goal.
2. Computes path lengths for:
   - Section Lines Along A*+ RRT*(Clothoid)
   - Section Lines Along A*+ RRT*(Clothoid)+ PR
3. Generates statistics (min, max, mean) for path lengths for each goal and path type.
4. Creates a boxplot comparing path lengths across goals and algorithms.
5. Saves the resulting boxplot in the `boxplot` folder with a timestamped filename.

Usage:
1. Place the `.pkl` files to analyze in the `test_pkl` folder.
2. Run the script. The boxplot will be generated and saved in the `boxplot` folder.

Output:
    - A timestamped boxplot saved in the `boxplot` folder.
    - Statistical summaries printed for each goal and path type.
"""

import pandas as pd
import numpy as np
import os
import matplotlib.pyplot as plt
import seaborn as sns
from SteerFunction import clothoid_path
import time

# Define the folder containing .pkl files
pkl_folder = 'test_pkl'

# Get all .pkl files in the test_pkl folder
pkl_files = sorted([os.path.join(pkl_folder, f) for f in os.listdir(pkl_folder) if f.endswith('.pkl')])

# Initialize the result dictionary
data = {'Goal': [], 'Path Type': [], 'Path Length': []}

# Process every 10 files as one goal
batch_size = 10
for goal_index, batch_start in enumerate(range(0, len(pkl_files), batch_size)):
    batch_files = pkl_files[batch_start:batch_start + batch_size]
    original_length_list = []
    refined_length_list = []

    for pkl_file in batch_files:
        # Load the DataFrame from the .pkl file
        df = pd.read_pickle(pkl_file)

        # Extract origin and refined nodes
        origin_nodes_list = df['origin_nodes_list'].tolist()
        refined_nodes_list = df['refined_nodes_list'].tolist()

        # Initialize path lengths for this file
        origin_total_length = 0
        refined_total_length = 0

        # Calculate path length for origin_nodes_list
        for i in range(len(origin_nodes_list) - 1):
            current_node = origin_nodes_list[i]
            next_node = origin_nodes_list[i + 1]

            # Skip if NaN
            if isinstance(current_node, float) and np.isnan(current_node):
                break
            if isinstance(next_node, float) and np.isnan(next_node):
                break

            # Extract points
            sx, sy, syaw, sk = current_node
            gx, gy, gyaw, gk = next_node

            # Compute clothoid path length
            _, _, _, path_length, _ = clothoid_path(sx, sy, syaw, sk, gx, gy, gyaw, gk)
            origin_total_length += path_length

        # Calculate path length for refined_nodes_list
        for i in range(len(refined_nodes_list) - 1):
            current_node = refined_nodes_list[i]
            next_node = refined_nodes_list[i + 1]

            # Skip if NaN
            if isinstance(current_node, float) and np.isnan(current_node):
                break
            if isinstance(next_node, float) and np.isnan(next_node):
                break

            # Extract points
            sx, sy, syaw, sk = current_node
            gx, gy, gyaw, gk = next_node

            # Compute clothoid path length
            _, _, _, path_length, _ = clothoid_path(sx, sy, syaw, sk, gx, gy, gyaw, gk)
            refined_total_length += path_length

        # Save results to the dictionary
        data['Goal'].append(f'Goal {goal_index + 1}')
        data['Path Type'].append('Section Lines Along A* + RRT* (Clothoid)')
        data['Path Length'].append(origin_total_length)
        original_length_list.append(origin_total_length)

        data['Goal'].append(f'Goal {goal_index + 1}')
        data['Path Type'].append('Section Lines Along A*+ RRT*(Clothoid)+ PR')
        data['Path Length'].append(refined_total_length)
        refined_length_list.append(refined_total_length)

    # Print statistics for the current goal
    print(f"Goal {goal_index + 1} Statistics:")
    print(f"  Origin (No LTR): min={np.min(original_length_list):.2f}, max={np.max(original_length_list):.2f}, mean={np.mean(original_length_list):.2f}")
    print(f"  Refined (With LTR): min={np.min(refined_length_list):.2f}, max={np.max(refined_length_list):.2f}, mean={np.mean(refined_length_list):.2f}")

# Convert results to a DataFrame
df_plot = pd.DataFrame(data)

# Generate boxplot
plt.figure(figsize=(8, 8))  # Adjust width and height of the figure
sns.boxplot(x='Goal', y='Path Length', hue='Path Type', data=df_plot, palette='Set2', showfliers=False)
plt.title('Comparison of Path Lengths Across Goals \n Using Different Algorithms', fontsize=16)
plt.xlabel('Goal', fontsize=12)
plt.ylabel('Path Length', fontsize=12)
plt.legend(title='Path Type')
plt.grid(axis='y', linestyle='--', alpha=0.7)
plt.tight_layout()  # Adjust layout to prevent overlap

# Save the boxplot to the `boxplot` folder with a timestamped name
os.makedirs('boxplot', exist_ok=True)
timestamp = time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime())
save_path = os.path.join('boxplot', f'boxplot_{timestamp}.png')
plt.savefig(save_path, format='png', dpi=300)
print(f"Boxplot saved as: {os.path.abspath(save_path)}")

# Show the plot
plt.show()
